// Have include guard.
#ifndef SELECTORCONTROLLER_H
#define SELECTORCONTROLLER_H

// Include files.
#include <string>
#include <map>
#include "WPILib.h"
#include "FpkException.hpp"
#include "RobotController.hpp"
#include "HumanDigitalInput.hpp"
#include "DriverStationDigitalInput.hpp"
#include "ValueOutput.hpp"

/**
 *  The SelectorController template class selects from a list items based on some input.
 *  
 *  Testing shows this class mostly works.
 *  
 *  @tparam ItemType Type of selectable. 
 */
template<typename Item>
class SelectorController :
	public RobotController
{
	public:
		typedef std::pair<std::string, Item> NameItemPairType;
	private:
		typedef typename std::vector<NameItemPairType>::size_type Size;
		
		// Has to be up here to compile.
		Size m_indexCurrent;
		std::vector<NameItemPairType> m_nameItemPairs;
		bool m_ownsInputs;
		HumanDigitalInput &m_inputUp;
		HumanDigitalInput &m_inputDown;
		bool m_inputsReleased;
		ValueOutput<std::string> &m_valueOutput;
	public:
		/**
		 *  Constructor for SelectorController.
		 * 	
		 * 	@param upInput Input for selecting up the list.  User is still reposible for deletion of parameter.
		 * 	@param downInput Input for selecting down the list.  User is still reposible for deletion of parameter.
		 */
		SelectorController
		(
			HumanDigitalInput &upInput, 
			HumanDigitalInput &downInput, 
			ValueOutput<std::string> &valueOutput
		) :
			m_indexCurrent(0),
			m_ownsInputs(false), 
			m_inputUp(upInput), 
			m_inputDown(downInput),
			m_valueOutput(valueOutput)
		{
		}
		
		/**
		 *  Constructor for SelectorController.
		 * 	
		 * 	@param driverStationIo Driver station IO.  User is still reposible for deletion of parameter.
		 *  @param buttonUp Channel number for button for selecting up the list.
		 *  @param buttonDown Channel number for button for selecting down the list.
		 */
		SelectorController
		(
			DriverStationEnhancedIO &driverStationIo, 
			UINT32 buttonUp, 
			UINT32 buttonDown,
			ValueOutput<std::string> &valueOutput
		) :
			m_ownsInputs(true), 
			m_inputUp(*(new DriverStationDigitalInput(driverStationIo, buttonUp))), 
			m_inputDown(*(new DriverStationDigitalInput(driverStationIo, buttonDown))),
			m_valueOutput(valueOutput)
		{
		}
		
		void AddItem(std::string name, Item item)
		{
			// We don't want to add items when selection is possible.
			if (!GetIsActive())
			{
				m_nameItemPairs.push_back(NameItemPairType(name, item));
				HandleItemAdded(item);
			}
			else
				throw FpkException("SelectorController: Can not add item while controller is active.");
		}
		
		Item GetSelectedItem()
		{
			return m_nameItemPairs[m_indexCurrent].second;
		}
		
		const std::vector<NameItemPairType> &GetNameItemPairs() const
		{
			return m_nameItemPairs;
		}
		
	protected:
		virtual ~SelectorController()
		{
			if (m_ownsInputs)
			{
				delete &m_inputUp;
				delete &m_inputDown;
			}
		}
		
		virtual void ActivateImpl()
		{
			m_indexCurrent = 0;
			
			if (!GetIsInvalidIndex(m_indexCurrent))
				HandleItemSelected(m_nameItemPairs[m_indexCurrent]);
		}
		
		virtual void DeactivateImpl()
		{
			if(!GetIsInvalidIndex(m_indexCurrent))
				HandleItemUnselected(m_nameItemPairs[m_indexCurrent].second);
		}
		
		virtual void Periodic()
		{
			try
			{
				bool isIndexInvalid = GetIsInvalidIndex(m_indexCurrent);
				
				if (m_inputsReleased && m_inputUp.Get() && !m_inputDown.Get())
				{
					if (m_indexCurrent > 0)
					{
						if (!isIndexInvalid)
							HandleItemUnselected(m_nameItemPairs[m_indexCurrent].second);
						
						m_indexCurrent--;
						
						HandleItemSelected(m_nameItemPairs[m_indexCurrent]);
						
						m_inputsReleased = false;
					}
				}
				else if (m_inputsReleased && m_inputDown.Get())
				{
					if (m_indexCurrent + 1 < m_nameItemPairs.size())
					{
						if (!isIndexInvalid)
							HandleItemUnselected(m_nameItemPairs[m_indexCurrent].second);
						
						m_indexCurrent++;
						
						HandleItemSelected(m_nameItemPairs[m_indexCurrent]);
						
						m_inputsReleased = false;
					}
				}
				else if (!m_inputUp.Get() && !m_inputDown.Get())
					m_inputsReleased = true;
			}
			catch (std::exception &e)
			{
				printf("Selector Controller: Exception! ");
				printf(e.what());
				printf("\n");
			}
			
			//printf("Current drive mode: %s\n", m_nameItemPairs[m_indexCurrent].first.c_str());
		}
		
		virtual void HandleItemAdded(Item item)
		{
		}
		
		virtual void HandleItemSelectedImpl(Item item)
		{
		}
		
		virtual void HandleItemUnselectedImpl(Item item)
		{
		}
	private:
		bool GetIsInvalidIndex(Size index)
		{
			return !(index >= 0) && (index < m_nameItemPairs.size());
		}
		
		void HandleItemSelected(NameItemPairType item)
		{
			m_valueOutput.SetValue(item.first);
			
			HandleItemSelectedImpl(item.second);
		}
		
		void HandleItemUnselected(Item item)
		{
			HandleItemUnselectedImpl(item);
		}
};

#endif // #ifndef SELECTORCONTROLLER_H 
